__all__ = ['SimulateMsg','PhysicalMsg']

import time
import socket
import queue
import threading
import warnings
import copy

from . import platform

SERVER_IP = '127.0.0.1'
IR_SENSOR_PORT = 41997
LOCATION_PORT = 41234
SAFE_CHECK_PORT = 41998

class UdpClient(object):
    _init_msg = "Initial: physical"
    _ip = SERVER_IP
    _port = None

    def __init__(self, port):
        self._port = port
        self._server_addr = (self._ip,self._port)

        self.query_lock = threading.Lock()
        self.query_event = threading.Event()
        self.query_id = -1
        self.query_type = ""
        self.query_reply = []

        self.to_send = queue.Queue()
        self.to_send.queue.clear()
        self.msg_lock = threading.Lock()
        
        self.term_id_lock = threading.Lock()
        self.term_id=0

        self.init_conn()
    
    def init_conn(self):
        self.ip_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.t_recv = threading.Thread(target=self.udp_recv)
        self.t_recv.daemon = True
        self.t_recv.start()

    def send_msg(self, msg, need_reply = False):
        tid = self._get_term_id()
        msg = str(tid) + ' ' + msg
        
        if (need_reply):
            print("[query]",msg)
            self.query_lock.acquire()
            self.query_event.clear()
            self.query_id = tid
            self.query_type = "position" if msg.find("position")!=-1 else "snapshot"
            
            # if (self.query_type == "snapshot"):
            #     print("snapshot acquire lock")
        
        self.ip_sock.sendto(msg.encode('utf-8'),self._server_addr)
        return tid

    def _get_term_id(self):
        self.term_id_lock.acquire()
        curr_id = self.term_id
        self.term_id = (self.term_id+1) % 1000
        self.term_id_lock.release()
        return curr_id

    def handle_query_reply(self,reply_data):
        if (self._port==IR_SENSOR_PORT):
            platform.SIMULATE_MSG.handle_query_reply(reply_data)
            return

        if (self.query_id==-1):
            return

        l = list(reply_data.split(' '))
        if (len(l)!=3):
            raise Exception("error recv msg = {}".format(reply_data))
        else:
            print(reply_data)
            pass
        tid = int(l[0])
        reply_type = l[1]
        msg = l[2]

        if (tid!=self.query_id
        or self.query_type.find(reply_type)==-1): 
            return

        self.query_type = reply_type

        legal = False
        if (reply_type=='position'):
            reply = msg.split(',')
            if (len(reply)==4):
                state = reply[0]
                x = float(reply[1])
                y = float(reply[2])
                deg = float(reply[3])
                if (state == "ActionAnalysis.UNMOVABLE"):
                    legal = True
                    self.query_reply = [x,y,deg]
        elif (reply_type=='snapshot'):
            reply = msg.split(',')
            if (len(reply)==4):
                state = reply[0]
                x = float(reply[1])
                y = float(reply[2])
                deg = float(reply[3])
                legal = True
                self.query_reply = [x,y,deg]
        
        if (legal):
            print("[reply]",reply_data)
            self.query_event.set()
            # if (self.query_type=='snapshot'):
            #     print("snapshot event set")
        else:
            print('[drop]',reply_data)

    def get_reply(self):
        # end_query_msg = "! "+self.query_type
        # self.send_msg(end_query_msg,need_reply=False)

        rep = self.query_reply[:]
        self.query_id=-1
        self.query_type=""
        self.query_reply.clear()
        if (self.query_type=='snapshot'):
            print("try to release lock")
        self.query_lock.release()
        return rep

    def udp_recv(self):
        self.ip_sock.sendto(self._init_msg.encode('utf-8'),self._server_addr)
        while True:
            try:
                data, ADDR = self.ip_sock.recvfrom(1024)
            except:
                print('远程主机已关闭 (addr)=',self._server_addr)
                break
            data = data.decode('utf-8')
            # print("[{2}] {0} \t  服务器端消息：{1}".format(
            #       time.asctime(time.localtime()), data,self._port))
            
            if (data=='quit'): break
            if (data=='ack'): continue

            self.handle_query_reply(data)
        
        # self.udp_send.kill()
        self.ip_sock.close()


class SensorSource:
    _tag = ""

    def __init__(self):
        self._status = False
        self._distance = [0] * 4
        self._yaw_angle = 0
        self._pitch_angle = 0
        self._yaw_ground_angle = 0
        self._pitch_ground_angle = 0
        self._cv2_image = None
    
    def set_online(self):
        self._status = True
    
    def is_online(self):
        return self._status

    def set_sensor_data_info(self, data_info):
        self._distance = data_info[:]
        # if (self._tag=='S'):
        #     d = self._distance
        #     print("[source-{}] tof1:{}  tof2:{}  tof3:{}  tof4:{}".format(self._tag, \
        #         d[0], d[1], d[2], d[3]))
    
    def set_angle_data_info(self, data_info):
        self._pitch_angle, self._yaw_angle, self._pitch_ground_angle, self._yaw_ground_angle = data_info[:]
        # print("[source-{}] angle:{}".format(self._tag, self._yaw_ground_angle))

    def get_sensor_data_info(self):
        return self._distance 

    def set_cv2_image(self, img):
        self._cv2_image = img

    def get_cv2_image(self):
        if self._cv2_image is None:
            return None
        return self._cv2_image

# Data from simulate 
 
class SimulateMsg(SensorSource):
    def __init__(self):
        super().__init__()
        self._tag = "S"

        self.sender = UdpClient(port=IR_SENSOR_PORT)
        self.set_online()
    
    '''
    explore for different action API:
        format: "? action [action_api] *args"
    
    1) move
        ? action move {x} {y} {z} {xy_spd} {z_spd}
    2) drive_speed #TODO
        ? action drive_speed {x_spd} {y_spd} {z_spd}
    '''
    def handle_query_reply(self,reply_data):
        reply_data = eval(reply_data)
        # print("SIM:",reply_data)
        F,B,L,R,time_tag,t = reply_data
        self.set_sensor_data_info([F,R,B,L])
        if (time_tag):
            time_gap = time.time()-t
            if (time_gap > 0.15): 
                s = "\ncycle: {:.6f}".format(time_gap)
                # warnings.warn(s,UserWarning)


        # self.set_sensor_data_info(reply_data)

    # def explore_action(self,msg):
    #     reply = self.sender.send_msg(msg, need_reply=True)
    #     if (len(reply)==7  and reply[0]=='!' and reply[1]=='action'):
    #         need_adjust = bool(reply[2])
    #         condition = [int(reply[3]),int(reply[4]),int(reply[5]),int(reply[6])]
    #         return need_adjust,condition
    #     else:
    #         raise Exception("error action reply = {}".format(reply))

    # def query_adjust(self):
    #     msg = "? adjust"
    #     reply = self.sender.send_msg(msg, need_reply=True)
    #     if (len(reply)==5 and reply[0]=='!' and reply[1]=='adjust' and reply[2]=='move'):
    #         x = float(reply[3])
    #         y = float(reply[4])
    #         z = float(reply[5])
    #         xy_spd = float(reply[6])
    #         z_spd = float(reply[7])
    #         return [x,y,z,xy_spd,z_spd]
    #     else:
    #         raise Exception("error positon reply = {}".format(reply))

class SecurityMonitor():
    def __init__(self):
        self._event = threading.Event()
        self._event.set()
        self._set_msg = 'initial'
        self._set_tid = -1
        self._monitor_distance = -1

        self._trigger_api = None
        pass

    def start(self,api='move',check_distance = 200):
        self._set_tid = -1
        self._event.clear()

        auto_stop_thread = threading.Thread(target=self._auto_stop)
        auto_stop_thread.start()

        self._set_msg = 'wait'
        self._trigger_api = api
        self._monitor_distance = check_distance

    def event_set_by(self,set_msg):
        print("monitor set by",set_msg)
        self._monitor_distance = -1
        self._set_msg = set_msg
        self._event.set()
        # print("set event")

    def is_trigger(self,min_distance):
        if (self._monitor_distance<0): return False
        if (min_distance<=self._monitor_distance and not self._event.isSet()):
            self.event_set_by('STOP')

            print("only trigger")
            return True
        return False
    
    def isSet(self):
        return self._event.isSet()

    def wait(self):
        return self._event.wait()

    def get_set_msg(self):
        while (self._set_msg=='STOP'): continue
        return self._set_msg

    def get_set_tid(self):
        tid = self._set_tid
        while (tid==-1):
            tid = self._set_tid
        return tid

    def set_tid(self,tid):
        print("cur tid =",tid)
        self._set_tid = tid

    def _auto_stop(self):
        # 如果是STOP set的，说明小车正在行驶且处于边界，
        # 需要主动停止车辆【设置速度为0
        self._event.wait()
        # print("stop wait success")
        if (self._set_msg=='STOP'):
            print("stop car")
            platform.EP_ROBOT.chassis._dij_drive_speed(0, 0, 0)
            self._set_msg='ADJUST'

    def adjust(self,api,obj,is_manual=False):
        platform.flush_undefined_behavior()
        print("adjust for",api)
        if api=='move':
            print(obj.percent_str)
        else:
            pass
        
        platform.adjust(platform.EP_ROBOT.chassis,is_manual)

class DriveSpeedAdjuster():
    def __init__(self) :
        self.cur_proto = None
        self.has_send = False
        self.proto_lock = threading.Lock()

        self.run_event=threading.Event()
        self.run_event.clear()

        self.run_proto_thread = threading.Thread(target=self.run_proto)
        self.run_proto_thread.daemon=True
        self.run_proto_thread.start()


        self.monitor_thread = None

    def proto_clear(self):
        self.proto_lock.acquire()
        self.cur_proto = None
        self.proto_lock.release()
        platform.flush_undefined_behavior()

    def run_proto(self):
        while True:
            if (self.cur_proto == None):
                print("wait next proto")
                self.run_event.wait()
                print("wait get")
                self.run_event.clear()
            
            platform.PHYSICAL_MSG.security_monitor.start('drive_speed',200)
            platform.PHYSICAL_MSG.security_monitor.wait()

            if (platform.PHYSICAL_MSG.security_monitor._trigger_api!='drive_speed'):
                # in a safe action, not do any thing
                self.proto_clear()
                continue

            set_reason = platform.PHYSICAL_MSG.security_monitor.get_set_msg()

            if (set_reason=='ADJUST'):
                tid = platform.PHYSICAL_MSG.security_monitor.get_set_tid()
            else:
                tid = None

            new_pos = platform.PHYSICAL_MSG.query_position(tid)

            print("set_reason =",set_reason)
            if (set_reason == "ADJUST"):
                self.drive_adjust()
                print("drive proto send again")
                self.send()
            elif (set_reason == "END"):
                self.proto_clear()

            print("---end _once_turn")
    
    def send(self):
        self.proto_lock.acquire()
        if (self.cur_proto):
            print("recover :x_spd:{0:f}, y_spd:{1:f}, z_spd:{2:f}".format(self.cur_proto._x_spd, self.cur_proto._y_spd, self.cur_proto._z_spd))

            platform.PHYSICAL_MSG.send_action_info(platform.encoder_proto(self.cur_proto))

            platform.EP_ROBOT.chassis._send_sync_proto(self.cur_proto)
        self.proto_lock.release()

    def register_proto(self,proto):
        self.proto_lock.acquire()
        self.cur_proto = proto
        self.has_send = True
        self.proto_lock.release()
        self.run_event.set()
    
    def drive_adjust(self):
        platform.PHYSICAL_MSG.security_monitor.adjust("drive_speed",self)

class PlatformTimerManager():
    def __init__(self):
        self.register_timer = []
        # self.queue_lock = threading.Lock()
        self.start_time = None
        self.adjust_lock = threading.Lock()

    def timer_register(self,timer):
        self.register_timer.append(timer)

    def timer_delete(self,timer):
        self.register_timer.remove(timer)

    def adjust_status_start(self):
        if not self.adjust_lock.acquire(blocking=False):
            print("already in adjust status")
            return False
        print("switch to adjust status")
        self.start_time = time.time()

        # into adjust status
        platform.EP_ROBOT.led.set_led(comp="all", r=200, g=200, b=0)
        platform.flush_undefined_behavior()

        print("1")

        for timer in self.register_timer:
            print(timer)
            timer.interval_lock.acquire()
        
        print("2")
        
        return True
    
    def adjust_status_end(self):
        print("exit adjust status")
        pos = platform.PHYSICAL_MSG.query_position()
        print("after adjust pos = {:.3f} {:.3f} deg = {:.3f}".format(pos[0],pos[1],pos[2]))

        # exit adjust status
        print("exit adjust status")
        platform.PHYSICAL_MSG.send_adjust_status(is_on=False)
        platform.EP_ROBOT.led.set_led(comp="all", r=0, g=200, b=0)

        interval_append = time.time() - self.start_time
        
        print("append time {:.3f}s".format(interval_append))
        for timer in self.register_timer:
            timer.append(interval_append)
            
        for timer in self.register_timer:
            timer.interval_lock.release()

        self.adjust_lock.release()


class PlatformTimer(threading.Thread):
    def __init__(self, interval, function, args=None, kwargs=None):
        threading.Thread.__init__(self)
        self.interval = interval
        self.interval_lock = threading.Lock()

        self.total_interval = interval
        self.start_time = time.time()


        self.function = function
        self.args = args if args is not None else []
        self.kwargs = kwargs if kwargs is not None else {}
        self.finished = threading.Event()

        platform.PLATFORM_TIMER_MANAGER.timer_register(self)

    def cancel(self):
        """Stop the timer if it hasn't finished yet."""
        self.finished.set()
        platform.PLATFORM_TIMER_MANAGER.timer_delete(self)
        
    def get(self):
        self.interval_lock.acquire()
        tmp = self.interval
        self.interval = 0
        self.interval_lock.release()
        return tmp

    def append(self,append_interval):
        self.interval+=append_interval
        self.total_interval += append_interval

    def get_left(self):
        spend_time = time.time()-self.start_time
        return min(0,self.total_interval-spend_time)

    def run(self):
        while True:
            wait_time = self.get()
            if wait_time == 0: break

            self.finished.wait(wait_time)
            print("get & wait =",wait_time)
            if (self.finished.is_set()):break

        if not self.finished.is_set():
            self.function(*self.args, **self.kwargs)
        self.finished.set()
        platform.PLATFORM_TIMER_MANAGER.timer_delete(self)

# Data from physical  
class PhysicalMsg(SensorSource):
    _sys_sub_modules = []
    # _chassis_mutex = threading.Lock()
    # _chassis_uuid = None
    # _chassis_api = []

    def __init__(self):
        super().__init__()
        self._tag = "P"
        self.sender = UdpClient(port=LOCATION_PORT)

        self.security_monitor = SecurityMonitor()

        # self._security_check_distance = -1
        # self._security_monitor_event = threading.Event()
        # self._security_set_msg = "None"
        # self._security_set_tid = -1

        # self._drive_api_event = threading.Event()

    def sys_add_sub_module(self,module):
        self._sys_sub_modules.append(module)

    def sys_sub_all(self):
        print("开启platfrom订阅")
        self.angle_init_once = False
        for module in self._sys_sub_modules:
            module.sys_sub()

    def sys_unsub_all(self):
        for module in self._sys_sub_modules:
            module.sys_unsub() 

    # def _auto_distance_security_check(self):
    #     # print("security_monitor wait")
    #     self._security_monitor_event.wait()
    #     # print("security_monitor isSet")
    #     if (self._security_set_msg == "STOP"):
    #         print("car speed set to 0")
    #         platform.EP_ROBOT.chassis._dij_drive_speed(0, 0, 0)

        # print("monitor_thread exit success")

    def set_sensor_data_info(self, data_info):
        super().set_sensor_data_info(data_info)
        F,R,B,L = self._distance

        min_distance = min(self._distance)
        stop_tag=self.security_monitor.is_trigger(min_distance)

        msg = "SENSOR_TYPE,{},{},{},{},{}{}".format(F,B,L,R,
            time.time(),
            ",STOP_TAG" if stop_tag else ""
        )

        tid = self.sender.send_msg(msg,need_reply=stop_tag)

        if stop_tag:
            print("stop tag")
            self.security_monitor.set_tid(tid)
            print(tid,msg)
        return 

    def set_angle_data_info(self, data_info):
        super().set_angle_data_info(data_info)
        a,b,c,d = data_info
        if (not self.angle_init_once):
            self.angle_init_once=True
            self.angle_init_val = d
            print("init angle to {}".format(self.angle_init_val))

        msg = "ANGLE_TYPE,{:.3f},{:.3f}".format(d-self.angle_init_val,d)
        # print(msg)
        self.sender.send_msg(msg,need_reply=False)
        # platform.SIMULATE_MSG.sender.send_msg(msg,need_reply=False)
        return

    def send_action_info(self,info):
        # action_info = "move,x,y,z,xy_spd,z_spd"
        msg = "ACTION_TYPE,{}".format(info)
        self.sender.send_msg(msg,need_reply=False)
        # platform.SIMULATE_MSG.sender.send_msg(msg,need_reply=False)
        return

    def explore_action(self,msg):
        reply = self.sender.send_msg(msg, need_reply=True)
        self.sender.send_msg(msg, need_reply=True)
        self.sender.query_event.wait()
        reply = self.sender.get_reply()

        return reply

    def query_position(self,tid=None):
        if (tid is None):
            msg="QUERY,position"
            self.sender.send_msg(msg, need_reply=True)
        else:
            # print("query_position",tid)
            pass

        # print("1")
        if (not self.sender.query_event.isSet()):
            self.sender.query_event.wait()
        # print("2")
        reply = self.sender.get_reply()

        return reply

    def location_server_reset(self):
        msg="SYSTEM_TYPE,RESET"
        self.angle_init_once = False
        self.sender.send_msg(msg, need_reply=False)
        return

    def send_adjust_status(self,is_on=True):
        msg="SYSTEM_TYPE,ADJUST,{}".format("on" if is_on else "off")
        self.sender.send_msg(msg, need_reply=False)
        return
